#include <stdio.h>
#include "position_cal.c"
#include "pid_cal.c"
#include "data_handler.c"
int main() {

    int j=50;


    filter_data_update(&filter1,1);
    filter_data_update(&filter1,2);

    for(j=0;j<99;j++){
        printf("j[%d] is %d\n",j,filter1.buff[j]);
    }

    pid_set_param(&pid_controller1,1.2,0.001,1.0);
    pid_init(&pid_controller1);
    pid_cal(&pid_controller1,1000);

    set_ini_pos(&current_pos,0.0,0.0);
    set_ini_angle(&current_pos,0);
    set_tar_pos(&target_pos,3.0,4.0);
    set_tar_angle(&target_pos,100);

    target_pos.length=length_cal(current_pos.x,current_pos.y,target_pos.x,target_pos.y);
    target_pos.angle=angle_cal();

    //printf("the angle is %d\nthe length is %f\n%d\n",target_pos.angle,target_pos.length,pid_controller1.output);

    return 0;
}